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Abstract — Filtering  algorithms  that  use  different  forms  of 
numerical  integration  to  handle  measurement  and  process  non- 
linearities,  such  as  the  cubature  Kalman  filter,  can  perform  ex¬ 
tremely  poorly  in  many  applications  involving  angular  measure¬ 
ments.  We  demonstrate  how  such  filters  can  be  modified  to  take 
into  account  the  circular  nature  of  the  angular  measurements, 
dramatically  improving  performance.  Unlike  common  alternate 
techniques,  the  cubature  methods  can  be  easily  used  with  angular 
measurements  arising  from  ray-traceable  propagation  models. 

I.  Introduction 

The  recent  tutorial  [9]  discusses,  considering  examples  of 
3D  target  tracking,  how  cubature/  unscented/  sigma  point 
Kalman  filtering  is  a  realization  of  the  best  linear  unbiased 
estimator  (BLUE)  that  evaluates  certain  integrals  for  expected 
values  using  different  forms  of  cubature  integration.  In  [9],  the 
term  “cubature  Kalman  filter”  is  generally  applied  to  all  such 
filters  to  highlight  the  use  of  cubature  integration,1  whereas 
in  [28]  the  term  “linear  regression  Kalman  filter”  is  used  to 
highlight  how  such  methods  can  be  viewed  as  a  form  of  linear 
regression.  This  paper  addresses  the  use  of  the  generic  family 
of  all  cubature  Kalman  filters2  (CKFs)  with  measurement  and 
dynamic  models  involving  angular  components  without  regard 
to  the  choice  of  cubature  points. 

Though  many  articles  on  various  types  of  cubature  Kalman 
filters  (CKFs)  consider  the  use  of  polar  [20],  [12],  [2]  or 
spherical  [34]  measurements  with  Cartesian  state  vectors, 
many  more  papers  that  consider  the  use  of  cubature  integration 
(usually  in  the  form  of  the  “unscented  transform")  consider 
just  converting  such  measurements  into  Cartesian  coordinates 
and  feeding  the  converted  measurements  to  a  standard  linear 
Kalman  filter,  such  as  in  [19],  [20],  [17],  [18],  [1],  [32], 
However,  in  [9],  it  was  demonstrated  that  when  tracking 
using  range  and  direction  cosine  measurements,  it  is  better 
to  use  them  in  a  CKF  than  to  to  use  unbiased  Cartesian- 
converted  measurements  for  tracking  and  one  would  expect 
similar  results  to  hold  when  using  measurements  in  spherical 
coordinates. 

Sometimes  the  term  ‘'quadrature”  is  used  in  place  of  cubature.  Usually,  the 
distinction  is  the  quadrature  integration  is  one-dimensional,  whereas  cubature 
integration  is  multidimensional.  However,  not  all  authors  make  the  distinction. 

2The  term  “cubature  Kalman  filter”  was  coined  in  the  paper  [2],  where 
third-order  cubature  points  were  used.  Many  authors,  when  discussing  the 
CKF  only  mean  the  CKF  with  that  particular  choice  of  cubature  points. 


However,  none  of  the  aforementioned  papers  considering 
the  use  of  polar  or  spherical  coordinates  in  a  CKF  takes 
into  consideration  the  idiosyncrasies  of  directional  statistics. 
In  various  monographs  on  directional  statistics  [30]  focussing 
on  circular  [13]  or  spherical  [14]  measurements,  problems 
with  traditional  definitions  of  the  mean  and  variance  are 
highlighted.  For  example,  in  numerous  examples  throughout 
[13],  the  problem  of  averaging  wind  directions  (in  the  local 
tangent  plane  to  the  surface  of  the  Earth)  is  considered.  If  one 
were  to  simply  numerically  average  the  direction  (given  as 
an  angle,  for  example.  North  of  East  or  East  of  North),  then 
averaging  a  value  just  above  0°  with  one  just  below  359° 
would  produce  the  worst  possible  estimate,  one  close  to  180° 
-exactly  opposite  the  direction  of  both  of  the  samples.  Thus, 
special  care  must  be  taken  when  computing  expected  values 
involving  numerical  data.  Consequently,  as  is  demonstrated  for 
polar  measurements  in  Section  III,  and  spherical  measurements 
in  Section  IV,  special  care  must  be  taken  when  computing  the 
expected  values  in  the  CKF  when  dealing  with  angular  models. 

Directional  statistics  have  been  used  in  various  aspects  of 
filtering  in  the  past.  For  example,  in  [26],  the  measurement 
update  step  of  a  particle  filter  whose  target  state  vector 
contains  an  angular  component  is  considered.  In  [36],  parts  of 
a  standard  Kalman  filter  used  to  estimate  angular  distributions 
are  wrapped  on  the  range  (0,  27t]  to  handle  special  nonlinear 
aspects  of  the  filter.  The  same  concept  is  applied  in  this 
paper  to  the  more  general  CKF  problems.  On  the  other 
hand,  a  cubature-style  filter  is  used  with  the  wrapped-normal 
and  von  Mises  distributions  for  estimating  an  angle  in  [25]. 
In  [24],  the  authors  consider  the  more  general  case  where 
multiple  angles  are  dependent,  such  as  when  using  spherical 
coordinates,  where  toroidal  distributions  and  moments  were 
used.  Though  the  algorithms  of  [25]  and  [24]  work  quite  well 
when  estimating  angular  quantities,  this  paper  considers  more 
general  estimation  problems  when  given  correlated  quantities 
other  than  angles,  for  example,  problems  involving  range  or 
Cartesian  position  in  the  state  and /  or  the  measurement  vectors. 
Whereas  in  [25],  [24],  solutions  utilizing  circular  probability 
distributions  are  derived,  this  paper  simply  demonstrates  how  a 
standard  linear  CKF  can  be  modified  to  accommodate  circular 
or  spherical  models  when  the  noises  involved  are  not  "too” 
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large. 

Though  the  CKF  is  not  suitable  for  all  estimation  problems, 
it  is  extremely  versatile  and  simple  to  use  in  many  challenging 
scenarios.  For  example,  in  the  tutorial  [8],  it  is  demonstrated 
that  the  CKF  can  handle  refraction-corrupted  measurements 
well  in  ray-traceable  atmospheric  models  without  having 
to  evaluate  derivatives.  Additionally,  since  the  measurement 
propagation  and  update  steps  in  the  CKF  are  separate,  one 
can  easily  be  replaced  by  another  algorithm  utilizing  Gaussian 
approximations.  For  example,  in  the  tutorial  [7],  the  CKF  mea¬ 
surement  update  step  is  paired  with  a  cubature-based  moment¬ 
matching  method  for  propagating  a  target  state  through  a 
nonlinear  stochastic  dynamic  model  with  non-additive  noise, 
as  opposed  to  the  derivative-based  nonlinear  state  propagation 
algorithm  introduced  in  [3]. 

Section  II  reviews  basic  aspects  of  the  measurement  update 
step  common  to  CKF  that  are  discussed  in  detail  in  [9]. 
Sections  III  and  IV  then  discuss  how  the  CKF  measurement 
update  step  can  be  modified  to  handle  polar  and  spherical 
measurements,  respectively.  The  results  are  summarized  in 
Section  V. 

II.  The  Basic  Form  of  Cubature  Kalman  Filters 

As  discussed  in  [9],  cubature  integration  is  based  on  the 
Fundamental  Theorem  of  Gaussian  Integration,  which  states 
that  the  definite  integral  of  any  polynomial  up  to  a  given 
degree  times  a  weighting  function  w  having  certain  properties 
can  be  determined  exactly  by  a  weighted  summation  of  the 
polynomial  evaluated  at  certain  fixed  points  based  on  the 
weighting  function.  As  an  equation,  for  a  scalar  integral,  this 
means  that 

/b  n 

w{x)g(x)dx  =  ^uoig(ii),  (1) 

i= o 

where  a  and  b  are  the  bounds  of  the  integral,  w(x)  is  the 
weighting  function,  g(x)  is  a  polynomial,  u>i  is  a  cubature 
weight  and  is  a  cubature  point  (sometimes  referred  to 
as  a  sigma-point  in  tracking  literature).  The  cubature  points 
and  weights  are  designed  to  be  valid  for  all  polynomials  up 
to  a  certain  degree.  For  polynomials  over  that  degree,  the 
integration  can  only  be  considered  an  approximation. 

A  monograph  by  Stroud  [35]3  discusses  aspects  of  cubature 
integration  for  many  different  weighting  functions  and  regions 
of  integration.  Common  scalar  weighting  functions  for  d- 
dimensional  multivariate  integrals  in  R  are  w(x)  = 
and  w{x.)  =  e^xH".  When  w(x)  =  1,  common  regions  of 
integration  are  the  unit  sphere  (or  hypersphere  in  more  than 
3  dimensions),  cube  (or  hypercube),  and  simplex  (a  triangle 
in  2D,  a  tetrahedron  in  3D,  and  similar  shapes  in  higher 
dimensions).  As  demonstrated  in  [9],  cubature  formulae  for 
the  weighting  function  w(x)  =  e^xH"  can  be  modified  to  work 


for  scenarios  when  one  is  performing  an  integral  of  a  function 
times  a  multivariate  normal  PDF  having  an  arbitrary  mean  and 
covariance.  In  other  words,  when  dealing  with  integrals  that 
arise  when  deriving  the  CKF.4 

Mathematicians  have  developed  numerous  cubature  formu¬ 
lae  as  tabulated  in  [35]  and  in  the  online  encyclopedia  of 
cubature  formulae  at  http://nines.cs.kuleuven.be/ecf/  described 
in  [6].  Many  versions  of  the  CKF  use  cubature  integration 
without  having  derived  it  from  the  mathetical  literature  on  the 
topic.  For  example,  while  the  formula  underlying  Arasaratnam 
and  Haykin’s  CKF  [2]  is  a  third-order  cubature  formula,  which 
was  derived  with  regard  to  the  mathematical  literature  on 
cubature  integration,  the  so-called  “unscented  transformation” 
in  the  unscented  Kalman  filter  [21],  [16],  [27]  is  a  third- 
order  cubature  formula  that  was  derived  independently  of 
past  mathematical  work.  Similarly,  the  recent  smart  sampling 
Kalman  filter  [33]  uses  a  stochastic  method  to  choose  points 
for  integration  that  are  de  facto  cubature  points. 

The  key  to  the  CKF  is  the  update  step,  which  is  an 
implementation  of  the  best  linear  unbiased  estimator  (BLUE) 
using  cubature  integration  to  evaluate  difficult  integrals.  The 
BLUE  is  described  in  the  context  of  target  tracking  (but  not 
cubature  integration)  in  [38],  where  solutions  for  tracking 
using  polar  measurements  and  using  spherical  measurements 
are  given  using  a  Taylor-series  approximation  assuming  that  no 
correlation  exists  between  the  noise  corrupting  the  components 
of  the  measurement.  In  practice,  however,  recursion  is  not 
necessary  as  the  algorithm  is  shown  to  perform  well  against 
other  techniques  in  [22], 

On  the  other  hand,  a  cubature-based  BLUE  can  handle 
more  general  scenarios,  such  as  when  correlation  between  the 
components  of  measurements  is  taken  into  account  or  when 
using  empirical  models  of  range-Doppler  coupling,  such  as  the 
model  of  [5].  The  estimation  step  in  the  BLUE/  CKF  is 

w  k 

, - - v 

£fc|fc  =£fc|fc-i  +  Pfc|fc-i  (zfc  —  zfc|fc-i)  i  (2) 

Pk\k  =Pfc|fc-i  -  Pfcffc— i  (p^ffc-i)"1  (PjSffc-i)\  (3) 

where  x.k^k_1  is  the  predicted  (prior  mean)  target  state  with 
covariance  matrix  Pfc|fc_!  (a  Gaussian  distribution  is  assumed) 
and  xfc|fc  is  the  updated  (posterior  mean)  target  state  with 
covariance  matrix  Pfc|*..  is  the  measurement,  and  the  term 
Wfc  is  known  as  the  “gain”  of  the  filter.  The  other  quantities 
are  given  in  terms  of  expected  values  as 


zfc|fc-i  —  E 

[/i(xfc,wfc)|  Z1;(/e_1^]  , 

Pfcfit-i  =E 

(xjfe  (z Z k\k—l ) 

Pfcffc-i  =E 

(z/c  Z k\k—l )  (z k  Zk\k-l)  | 

Zl:(jfc— 1)J 

3Stroud's  extensive  monograph  on  cubature  integration  [35]  is  out  of  print. 
Since  the  book  was  printed  in  1971  and  U.S  copyright  on  a  work  published 
in  1971  is  valid  95  years  from  the  copyright  date  [37],  the  copyright  will  not 
expire  until  2066. 


4Cubature  formulae  for  the  weighting  function  tu(x)  =  ellxH  could 
be  similarly  transformed  to  work  with  the  Laplace  distribution.  Cubature 
formulae  for  iu(x)  =  1  over  regions  of  various  shapes  could  be  used  for 
evaluating  polynomial  integrals  over  uniform  distributions  of  various  shapes. 
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where  the  conditioning  Z1:(fc_!)  is  on  all  prior  information 
(which  is  assumed  to  result  in  a  Gaussian  prior  distribution), 
and  /i(xfc,  Wfc)  is  the  function  that  transforms  that  state  x*,  into 
the  measurement,  including  Gaussian  measurement  noise  w*., 
which  could  be  additive  or  included  in  a  nonlinear  manner.  To 
simplify  the  discussion  in  this  paper,  the  measurement  noise 
is  assumed  additive.  That  is  h(xk)  +  is  used  in  place  of 
/i(xfc,  Wfc).  However,  the  concepts  of  this  paper  can  be  applied 
to  general  noise  models  as  in  Section  IX. C  of  [9].  In  [38], 
the  BLUE  problem  is  formulated  slightly  differently  so  that 
the  measurement  is  converted  into  the  target  state  (Cartesian) 
domain,  whereas  here,  zk\k-i  is  the  conversion  of  the  target 
state  into  the  measurement  domain. 

Though  cubature  estimation  techniques  have  been  developed 
for  circular  probability  distributions  such  as  in  [23],  there 
are  no  cubature  estimation  methods  explicitly  for  measure¬ 
ment  models  where  only  some  of  the  components  of  the 
measurement  are  circular,  such  as  when  given  a  range  and 
an  azimuth  measurement.  Thus,  this  paper  demonstrates  how 
linear  models  can  be  adapted  to  such  applications. 

Note  that  the  assumption  that  the  noise  corrupting  the 
measurement  is  Gaussian-distributed  is  kept  here.  Strictly 
speaking,  the  assumption  regarding  noise  corrupting  the  an¬ 
gular  component  is  best  described  as  wrapped  normal,  which 
is  described  in  [30,  Ch.  3.5.7].  By  approximating  the  noise 
corrupting  the  measurement  as  being  multivariate  normal,  it 
is  possible  to  approximately  model  cross-correlations  between 
angular  and  linear  components.  Also  note  that  if  the  noise 
variance  on  the  angular  component  is  small,  standard  methods 
(possibly  modified  with  a  modulo  operation)  can  be  used  to 
approximately  estimate  the  covariance  matrix  of  the  measure¬ 
ments  from  measurement  and  truth  data.  This  becomes  clear 
when  examining  the  (scalar)  clipped  mod  normal  distribution 
used  in  [11],  which  differs  very  little  from  a  standard  normal 
distribution,  but  which  can  be  applied  to  circular  data  without 
explicit  wrapping. 

III.  Using  Polar  Measurement  in  a  Cubature 
Filter 

A.  The  Method 

In  the  standard  CKF,  one  would  expect  problems  to  arise 
when  taking  the  differences  of  the  angular  components  in 
(2)  when  computing  xfc|fc,  as  well  as  in  (5)  and  (6),  when 
computing  P^k_1  and  P^k_v  due  to  the  difference  between 
angles  being  discontinuous  at  the  0  —  2-7T  boundary  in  the 
angular  components  of  the  innovation  term  zk  —  ik\k-i  and  the 
z  —  z k\k-i  terms.  Additionally,  the  evaluation  of  the  expected 
value  in  (4)  to  compute  the  predicted  measurements  involves  a 
conversion  of  the  target  state  into  the  coordinate  system  of  the 
measurement.  When  given  measurements  in  polar  coordinates 
and  a  target  state  in  Cartesian  coordinates,  however,  problems 
related  to  averaging  angles  can  arise. 

When  finding  the  mean  and  variance  of  circular  values,  the 
mean  direction  pg  is  typically  used  in  place  of  the  arithmetic 
mean  and  the  circular  variance  ag  in  place  of  the  more 


traditional  standard  deviation.  Given  an  angle  9,  the  mean 
direction  and  circular  standard  deviation  are  generally  defined 
in  terms  of  average  components  ux  and  uy  of  an  algebraic 
average  of  unit  vectors5  u  as  well  as  the  mean  resultant  length, 
p  such  that 

ux  =  E  [cos  6}  uy  =  E  [sin  6] .  (7) 

The  mean  resultant  length,  p,  and  mean  direction,  jig,  are 
defined  through  the  polar  relation  [30,  Ch.  2.2] 

Ux  +  jUy  =  pe ,  (8) 

where  j  =  1.  Thus, 

p  =  J u ^  pe  =  arctan2  (uy,ux) ,  (9) 

where  arctan2  refers  to  a  four-quadrant  inverse  tangent  func¬ 
tion.  The  circular  standard  deviation  is  defined  as  [30,  Ch. 
2.3] 

(Tg  =  — 2  In  p  (10) 

When  deriving  an  angle-only  recursive  filtering  algorithm 
in  [25],  the  authors  made  use  of  such  circular  statistics  in  the 
context  of  circular  probability  distributions.  However,  here, 
we  consider  the  more  general  problem  of  handing  both  linear 
(range)  and  circular  (angle)  measurement  components,  which 
could  possibly  be  correlated.  In  [30,  Ch.  11.1],  the  concept 
of  a  cross  correlation  between  circular  and  linear  terms  is 
defined  and  in  [30,  Ch.  1 1 .3]  the  topic  of  performing  regression 
between  circular  and  linear  quantities  is  broached.  However, 
the  regression  technique  is  not  sufficiently  well  developed  to 
completely  replace  the  CKF  and  it  would  be  inappropriate 
to  replace  and  P^k_1  with  components  having  fully 

circular  terms  as  long  as  the  design  of  the  overall  measurement 
update  remained  linear. 

Even  though  a  version  of  the  CKF  for  mixed  circular 
and  linear  variables  is  not  trivial  to  derive,  one  can  assume 
that  a  simple  modification  to  a  linear  estimator  would  work 
well.  For  example,  in  [10],  when  considering  range  ambiguity 
resolution,  which  results  from  aliasing  in  range  measurements 
(a  circular  property),  it  is  demonstrated  that  a  simple  algorithm 
based  on  a  linear  model  works  nearly  as  well  as  an  optimal 
algorithm,  because  there  is  not  much  noise  in  a  realistic  sys¬ 
tem.  In  the  problem  at  hand  here,  simple  ad-hoc  corrections  to 
eliminate  the  0  —  27r  discontinuity  can  eliminate  any  problems. 

Thus,  the  expected  value  in  (4)  is  not  implemented  in  the 
traditional  manner  described  in  [9],  which  is 

Nc 

z/c|fc-i  =  ^  w*Ci>  (id 

2  =  0 

where 

C i  —  h{x.k  |£_i  +  P||fc_1Ci)  (12) 

is  the  ith  cubature  weight  out  of  a  total  of  Nc  that  has  been 
transformed  to  model  the  moments  of  the  noisy  target  state 
after  measurement  prediction,  and  the  matrix  square  root  in 

5The  algebraic  mean  of  unit  vectors  is  usually  not  a  unit  vector. 
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(12)  should  be  a  lower-triangular  Cholesky  decomposition. 
Rather,  if  the  measurement  function  contains  a  circular  com¬ 
ponent,  then  that  has  to  be  averaged  separately  from  the  other 
components  in  the  manner  of  (9).  Note  that  in  (12),  h  is  the 
polar  measurement  function 

/i(x)  =  [yj x2  +  y2,  arctan2(j/,  x)]' .  (13) 

For  the  case  where  one  has  2D  range  and  position  compo¬ 
nents  the  steps  are 


1)  Compute  cubature  points  for  the  distribution  of  the 
predicted  measurement  in  the  manner  normally  done 
for  cubature  integration.  That  is,  use  (12).  The  cubature 
point  £ l  =  [Ci’r,Ci’  ]'  is  composed  of  a  range  compo¬ 
nent  and  an  angular  component  £?’  . 

2)  The  mean  of  the  range  components  is  found  the  standard 
way  as  a  linear  sum 

Nc 

^fc|fc-l  =  '  (14) 

i= 0 


3)  The  mean  angular  component,  on  the  other  hand,  is  the 
weighted  average  of  the  direction  vectors  (the  sample 
mean) 


79, 

^ k\k - 


\  / 

_sm  [Ci  )  _ 

(15) 

Nc 

(16) 

i= 0 

arctan2  (uy,  ux) . 

(17) 

In  other  words,  the  cubature  integration  is  used  to 
approximately  evaluate  the  integrals  in  (7). 

4)  The  predicted  measurement  is  now  ik\k-X  — 
[*k|fc-i>*k|k-i]  • 


On  the  other  hand,  when  evaluating  the  innovation  term 
zfc  —  zfc|fc-i  in  (2),  as  well  the  angular  differences  in(5),  (6) 
for  P^ffc_1  and  P^fc_1  via  cubature  integration,  an  ad-hoc 
method  of  dealing  with  the  circular  nature  of  the  azimuth 
component  of  the  measurement  is  to  realize  that  all  differ¬ 
ences  between  angles  must  be  bound  between  ±7r.  Thus,  all 
differences  between  angles  should  be  wrapped  into  that  region. 
To  wrap  a  value  to  the  region  (a,  6),  where  a  <  b,  one  can 
use  the  function 

hwmp(x,  a,  b )  =  mod(x  —  a,  b  —  a)  +  a,  (18) 

where  mod  refers  to  the  modulo  operation  (the  mod  func¬ 
tion  in  Matlab).  For  example,  mod  (12,5)  =  2.  Thus, 
equation  (2)  becomes 

+  Pfcffc-i  (Pfc|fc-l)  frpcZ  (Zk  ~  Zfc|fc-t)  , 

(19) 


where  for  some  difference  parameter  Az  =  [Ar,  A 9]' , 

A  r 

hWrap(A6»,—  7T,7T)  ' 

Using  the  wrapping  function  (5)  and  (6)  are  similarly 
evaluated  as 

1)  Compute  cubature  points  for  the  distribution  of  the 
predicted  state  in  the  manner  normally  done  for  cubature 
integration.  That  is,  use  (12)  to  get  C,\  =  [C*’r> 

Also,  save  the  non-transformed  cubature  points, 

C i  =  *-k\k-l  +  Pfc|fc  — lCi-  (21) 

2)  Pfcjfe—j  and  PfcjT.j  are  evaluated  as 

Nc- 1 

pk\k~i=  J2ui(Ci-*k  ifc-0  (ipoii  (cz  -  zfcifc-t>  ~7ri7rY  i 
i= 0 

(22) 

Nc- 1 

p^-i=  (cr  -  Zfcifc-O  h;z  (c  ?  -  ^ )'■ 

i=0 

(23) 

B.  An  Example 

As  an  example,  consider  the  problem  of  tracking  a  target 
moving  with  a  nearly-constant  velocity  in  two  dimensions. 
This  is  a  2D  order- 1  version  of  the  generalized  model  given 
in  Appendix  A.  Under  this  model  the  target  state  has  the  form 
x  =  [x,y,  x,y\'  for  2D  position  (x,y)  and  velocity  ( x,y ) 
components. 

Two  scenarios  are  considered.  The  initial  target  states  in  the 
scenarios  are 

x(,0)  =  [100  km  0  —200  m/s  0]',  (24) 

Xq1}  =  [  —  100  km  0  200  m/s  0]'.  (25) 

In  both  instances,  the  target  is  heading  toward  the  origin 
(where  the  sensor  is  located),  but  the  target  is  on  either  side  of 
the  y- axis.  For  polar  measurements,  where  the  range  ranges 
±7r  measured  counterclockwise  from  the  x-axis,  this  means 
that  one  would  expect  the  traditional  CKF  to  have  significant 
problems  with  the  track  having  initial  state  XqX\ 

The  simulations  are  run  assuming  that  the  components  of  the 
Gaussian  noise  corrupting  the  range  and  angle  measurements 
are  uncorrelated  with  range  standard  deviation  oy  =  20  m 
and  angular  standard  deviation  ag  =  0.5°.  The  time  between 
measurements  is  a  constant  T  =  3  s.  The  process  noise 
intensity  parameter  is  q  =  1  m2/s3. 

The  simulation  is  run  using  four  filters.  The  first  is  a  stan¬ 
dard  Kalman  filter,  which  uses  uses  the  first-two  moments  of 
the  Cartesian  converted  measurements  computed  using  ninth- 
order  cubature  integration,  as  done  is  in  [9] .  However,  since  the 
fifth-order  cubature  relations  given  in  [9]  require  points  to  have 
three  or  more  dimensions,  the  arbitrary-order  1-dimensional 
routine  of  [15]  is  used  with  the  product  rule  of  [35,  Ch.  2]  to 
make  it  two-dimensional.  The  second  filter  is  the  approximate 
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Fig.  1.  A  comparison  of  the  different  filtering  algorithms.  In  (a)  and  (b)  the  RMSE  performance  of  the  different  filtering  algorithms  is  shown.  In  (a),  where 
the  initial  target  state  is  on  the  right  of  the  y- axis,  far  from  the  dz7r  boundary  in  polar  angle,  all  of  the  filters  perform  similarly,  though  if  plotted,  one  would 
see  that  the  average  normalized  estimation  error  squared  (ANEES)  performance  of  the  converted  measurement  filter  is  somewhat  less  statistically  consistent. 
However,  in  (b),  where  the  initial  target  state  is  on  the  left  of  the  y  axis,  near  the  ±7r  boundary,  the  performance  of  naive  CKF  is  extremely  poor.  Observing 
a  sample  Monte  Carlo  run,  one  would  see  that  the  naive  filter  experiences  wild  gyrations;  the  proper  CKF  implementation  of  this  paper  does  not. 


BLUE  filter  of  [38],  which  avoids  problems  with  angles  by 
performing  the  measurement  update  step  with  a  different 
formulation  using  Cartesian  coordinates.  The  third  filter  is  a 
naive  CKF  with  ninth-order  cubature  points,  implemented  as 
described  in  [9]  without  accounting  for  the  circular  nature 
of  the  measurements.  The  fourth  filter  is  a  “proper”  CKF, 
which  has  been  adjusted  as  described  in  this  paper  to  avoid 
problems  with  measurements  near  a  ±7r  boundary.  The  two 
implementations  of  the  CKF  use  the  same  fifth-order  cubature 
points  as  used  in  the  simulations  in  [9]. 

Figures  la  and  lb  show  the  root- mean-squared  error  per¬ 
formance  of  the  filters  in  the  two  different  scenarios.  In  la, 
when  all  of  the  filters  are  far  from  the  ±7r  boundary,  the 
performance  is  comparable,  though  the  average  normalized 
estimation  errors  squared  (ANEES)  of  the  filters  (a  measure 
of  the  consistency  of  the  covariance  matrix  with  actually 
observed  error,  discussed  in  [9])6  are  comparable,  except 
for  the  converted  measurement  filter,  which  more  frequently 
leaves  the  95%  confidence  interval  (not  plotted). 

In  lb,  when  near  the  boundary,  the  naive  CKF  has  extremely 
poor  performance.  Viewing  a  sample  Monte  Carlo  of  the  poor 
scenario  in  Figure  1,  one  would  see  that  the  naive  CKF  has 
wild  jumps.  On  the  other  hand,  when  far  from  the  boundary, 
the  performance  of  the  filters  is  comparable. 

The  proper  CKF  avoid  the  problems  of  the  naive  CKF 
having  comparable  performance  and  statistical  consistency  as 
the  approximate  BLUE  estimator  of  [38].  However,  it  should 
be  noted  that  unlike  the  BLUE  estimator  of  [38],  the  CKF 
can  be  used  with  polar  measurements  originating  from  other 
propagation  models.  That  is,  the  measurement  function  in  the 
filter  can  include  some  type  of  ray  tracing  or  other  effects. 
For  example,  the  measurement  could  consist  of  a  2D  bistatic 

6The  ANEES  is  a  less  reliable  measure  of  the  accuracy  of  a  covariance 
matrix  than  the  noncredibility  index  and  the  inclination  indicator  of  [29],  but 
it  is  easier  to  understand  at  a  glance  than  using  two  different  measures. 


range  and  angle.  The  filter  of  [38]  has  only  been  formulated 
for  the  monostatic,  refraction-free  scenario. 

IV.  Using  Spherical  Measurements  in  a  Cubature 
Filter 


A.  The  Method 


When  given  measurements  in  spherical  coordinates,  the 
same  approach  as  in  Section  III-A  is  taken  to  approximate  the 
integrals  in  (4),  (5),  and  (6)  and  to  deal  with  the  differencing 
of  direction  components  in  (2).  That  is,  find  the  average 
direction  instead  of  the  linear  mean,  when  averaging  angular 
components  to  get  zk\k_1,  and  wrap  all  differences  between 
angles  to  the  appropriate  region  in  spherical  space. 

The  evaluation  of  z k\k-i  is  straightforward  since  the  aver¬ 
age  direction  in  spherical  coordinates  is  a  weighted  set  of  unit 
vectors.  Note,  however,  that  the  computation  of  the  unit  vector 
varies  depending  on  how  spherical  coordinates  are  defined. 
For  example,  a  common  representation  of  a  point  in  spherical 
coordinates  is  ( r,9,<f> ),  where  9  is  an  azimuth  measured  in 
the  x  —  y  plane,  as  radians  counterclockwise  from  the  x  axis 
and  <f>  is  an  elevation  above  the  plane.  In  such  an  instance,  the 
equations  for  a  point  (x,  y ,  z)  in  Cartesian  space  given  a  point 
in  spherical  coordinates  is 


x  =  r  cos(9)  cos(^)  y  =  r  sin(0)  cos(4>)  z=r  sin(</>). 

(26) 

The  inverse  transformation  is  consequently, 

r  =y/x2  +  y2  +  z 2  9  =  arctan2  (y,  x)  <j>  =  arcsin  Q  . 

r(27) 

To  find  unit  vectors  for  averaging,  it  given  a  Cartesian  state 
with  position  components  xp  =  [x,  y,  z]',  a  unit  vector  up  is 
simply 

up  =  (28) 
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When  given  angular  components,  a  unit  vector  u9  can  be  found 
by  evaluating  (26)  with  r  =  1. 

The  steps  to  find  ik\k-i  are  consequently, 

1)  Compute  the  transformed  cubature  points  = 

[£>r,u?]'  using  (12),  where  the  function  h  computes 
the  range  as  in  the  transformation  from  Cartesian  to 
spherical  coordinates  in  (27),  but  the  unit  direction 
vector  is  computed  using  with  the  position  components 
of  the  cubature  point  28. 

2)  The  mean  of  the  range  components  is  found  the  standard 
way  as  a  linear  sum, 

Nc 

^ik-i  =  Yluji&r-  (29) 

i= 0 


3)  Average  3D  unit  vectors  for  all  of  the  cubature  points: 

Nc 

(30) 

i= 0 

Zfc|fc_i  J=arctan2  {uy,ux) ,  (31) 

€|fc-i  =arcsin(pf)  •  (32) 

where  u  =  [ux,uy,uz]' . 

4)  The  predicted  measurement  is  now  ik\k-i  = 

Izr  76  Z *  1' 

Ufc|fc-l’  *k\h-V  • 

Similarly  to  how  the  differences  in  Section  III-A  for  the 
polar  coordinates  are  wrapped  to  the  circle  when  computing 
Pfcffc-i  and  Pfcffc-i  from  (5)  and  (6)  as  well  as  the  difference 
in  (2),  they  will  be  wrapped  to  the  sphere  in  this  instance. 
To  wrap  the  differences  to  the  sphere,  note  that  differences 
in  azimuth  and  elevation  can  only  vary  in  the  range  (±7t). 
However,  if  the  elevations  are  limited  to  the  range  ±tt/2,  then 
there  will  be  no  issues  (i.e.  with  directional  statistics)  taking 
differences  of  points  at  the  ±7t/2.  Thus,  it  is  assumed  that  all 
points  are  first  wrapped  such  that  the  elevation  is  in  the  range 
±tt/2.  Though  normally  not  an  issue.  Appendix  B  says  how 
to  perform  this  type  of  wrapping. 

Thus  (2)  for  spherical  measurements  becomes 

xfc|fc  =  Xfcifc-r  +  Pfcf, k-i  (Pfcffc-i)  KpZ  (zfc  “  zfc|fc_i), 

(33) 

where  for  some  difference  parameter  Az  =  [Ar,  A0]', 


hWraP(6>,_  7T,7T) 

0 


(34) 


The  algorithm  for  finding  P^k_1  and  Pzk\k_1  for  spherical 
measurements  is  then  very  similar  to  the  algorithm  for  the  case 
with  polar  measurements: 

1)  Compute  cubature  points  for  the  distribution  of  the  pre¬ 
dicted  state  in  the  manner  normally  done  for  cubature  in¬ 
tegration.  That  is,  use  (12)  to  get  Q  =  [Ci’r ,Ci’B Xi'^Y 
where  6  (±7t/2).  Normally,  nothing  extra  has  to 


be  done  to  keep  in  the  proper  range.  Also,  save  the 
non-transformed  cubature  points, 

C i  ^  Xfc|fc-i  +  Pfc|fc^iCi-  (35) 

2)  Pfcffc_i  and  P^fc_1  are  evaluated  as 

Nc- 1 

Pfcffc-t=Ewi  (Cf  -  Xfcifc-r)  (, Ct  ~  ik\k-u 

i=0 

(36) 

JVc-l 

p^-i =£<*«(«  -  KZict  -  Zfcifc-i)'- 

i=0 

(37) 

where  for  some  difference  parameter  z  =  [r,  9,  </>]', 

B.  An  Example 

To  demonstrate  the  necessity  and  limitations  of  the  wrap¬ 
ping  algorithms  four  scenarios  are  considered,  each  differeing 
in  the  initialization  used.  The  initializations  are 

x[,0)=  [100  km  0  0  -200  m/s  0  0]',  (38) 

x^1}  =  [  -100  km  0  0  200  m/s  0  0]\  (39) 

Xq2^  —  [0  0  100  km  0  0  -200  m/s]',  (40) 

Xq3' =  [a  0  a  b  0  6]',  (41) 

where 

/(100)2  / 2002  , 

a  — - —  km  b  =  -  -p-  m/s.  (42) 

In  all  instances,  the  target  starts  at  the  same  distance  and 
speed,  heading  toward  the  origin.  However,  with  Xq°\  the 
target  is  far  from  the  spherical  poles  and  the  ±7r  boundary 
in  azimuth;  in  Xg1^  it  is  near  the  ±tt  boundary  in  azimuth,  but 
far  from  the  poles;  in  xi2^  it  is  directly  at  the  poles,  and  in 
Xg  ,  it  is  at  a  45°  elevation  and  far  from  the  ±7r  boundary  in 
azimuth. 

The  same  dynamic  model  is  used  as  in  Section  III-B, 
generalized  to  three  dimensions,  and  range  and  azimuth  and 
elevation  standard  deviations  are  respectively  ar  =  20  m  and 
O-0  =  cr^  =  0.5°,  which  is  the  same  range  and  azimuth 
accuracy  as  in  Section  III-B. 

Figure  2  shows  the  RMSE  performance  of  the  four  filters 
previously  considered  in  the  polar  case,  with  the  approximate 
BLUE  filter  for  spherical  coordinates  taken  from  [38].  For  the 
measurement  conversion  and  cubature  filters,  the  same  fifth- 
order  cubature  points  were  used  as  in  [9].  As  expected,  all  of 
the  filters  have  comparable  performance  in  Scenario  0,  which 
is  far  from  any  boundaries  and  is  not  plotted,  but  in  2a,  near  the 
azimuthal  ±7r  boundary,  the  naive  CKF  performs  very  poorly. 
On  the  other  hand,  when  the  trajectory  starts  at  90°  elevation 
in  2b,  all  of  the  filters  except  for  the  converted  measurements 
Kalman  filter  perform  very  poorly.  For  the  cubature  filters,  this 
can  be  explained  by  the  fact  that  at  exactly  90°,  the  azimuth 
coordinate  provides  no  information,  but  the  (linear)  method  of 
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Fig.  2.  RMSE  performance  of  the  four  filters  under  consideration  when  tracking  in  spherical  coordinates.  In  Scenarios  0  and  3,  the  performance  of  all  of  the 
filters  is  similar  and  is  not  shown.  In  (a),  the  target  is  near  the  ±7r  boundary  in  azimuth.  In  (b),  the  target  starts  at  90°  elevation  and  goes  down  towards  the 
sensor. 


computing  the  correlation  matrices  in  (5)  does  not  assign  zero 
correlation  terms  due  to  artifacts  of  wrapping.  In  Scenario  3 
(not  plotted),  the  filters  all  appear  to  again  be  equivalent. 

V.  Conclusions 

Corrections  adapting  CKFs  to  polar  and  spherical  measure¬ 
ments  were  provided,  eliminating  extremely  bad  performance 
seen  in  certain  regions  with  uncorrected  filters.  In  the  polar 
scenario,  the  corrected  CKFs  had  similar  performance  when 
compared  to  an  approximate  BLUE  estimator  and  were  more 
consistent  than  using  a  converted-measurement  Kalman  filter. 
In  the  spherical  scenario,  the  corrected  CKF  was  found  to  be 
more  consistent  that  the  converted  measurement  filter  or  the 
BLUE  at  moderate  elevation  angles.  With  proper  corrections 
for  the  circular  nature  of  the  components,  the  CKF  can  handle 
spherical  and  polar  measurements  arising  from  general  ray- 
traceable  measurement  models  as  in  [8],  making  the  routine 
more  generally  applicable  than  the  approximate  BLUE  esti¬ 
mators. 
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Appendix  A 

A  General  Discretized  Nearly-Constant  Linear 
Dynamic  Model 

The  tutorial  [7]  discusses  continuous-time  stochastic  dy¬ 
namic  model.  A  linear  time-invariant  stochastic  dynamic 
model  has  the  form 

dxt  =  A  xtdt  +  Ddvt,  (43) 

where  xt  is  the  continuous-time  state  with  differential  dxt  (for 
a  differential  time  increment  dt),  dvt  is  the  differential  Wiener 
noise  process,  and  A  and  D  are  constant  matrices. 


For  one  dimensional  motion  with  an  nth  order  model,  with 
n  =  1  being  nearly  constant  velocity,  n  =  2,  nearly  constant 
position,  and  so  on,  the  matrices  are 


A  = 


0  for  n  =  0  (the  scalar  case) 

On,l  In,n 

0  olin 


for  n  >  0 


D  = 


£*n,l 

1 


(44) 

(45) 


where  q  is  the  process  noise  intensity.  The  A  matrix  just 
integrates  the  moments  upwards  (velocity  to  position),  and  the 
D  matrix  injects  noise  into  the  model.  This  implies  that  dwt 
is  scalar. 


The  mathematics  for  discretizing  such  a  system  are  de¬ 
scribed  in  [4,  Ch.  6.2]  and  [31,  Ch.  4.9].  If  the  interval  between 
discrete-time  step  k  and  k  +  1  is  T  seconds,  the  discretized 
dynamic  model  simplifies  to 

xfc+i  =  F(T)xfc  +  vfc,  (46) 


where  the  covariance  matrix  of  the  discrete-time  noise  is 
Q  (T).  The  elements  in  row  r  and  column  c  of  the  matrices 
F(T)  and  Q(T)  are 


if  c  —  r  >  0 


{rpc-r 

Jc--r)\ 

0  otherwise 

'■y  (n — r)  +  (n — c) + 1 

[Qcn]Lc=7 


(n  —  r)!(n  —  c)!((n  —  r)  +  (n  —  c)  +  l) 


(47) 


q2.  (48) 


To  extend  the  model  to  multiple  dimensions,  multiple  inde¬ 
pendent  1  -dimensional  dynamic  models  can  be  stacked.  If  the 
target  state  is  should  be  in  n-dimensional  Cartesian  space  con¬ 
sisting  of  components  x  =  [xi, .  ■ . ,  xn ,  Xi, . . . ,  icn,Xi  . . .]', 
where  a  dot  above  the  variable  represents  a  derivative,  then 
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Q  n\T)  =Q(T)®In,n 


F"(T)  =F(T)®I„,„ 


(49) 


where  Q n(T)  and  F n(T)  are  the  process  noise  covariance 
matrix  and  state  transition  matrix  for  ?r-dimensions,  I„jn  is 
the  n  x  n  identity  matrix  and  ®  is  the  Kronecker  product 
operator,  which  is  the  kron  command  in  Matlab. 

Appendix  B 

How  to  Wrap  with  Mirroring 
Given  a  direction  in  spherical  coordinates  (0,0),  where  6 
is  azimuth  and  0  is  elevation,  but  to  which  offsets  have  been 
added  so  that  9  is  no  longer  necessarily  in  the  range  of  ±7r 
radians  and  0  is  no  longer  in  the  range  ±7t/2  radians,  the 
point  can  be  mapped  back  to  the  sphere  as  follows: 

If  mod  ^  2  ^ ,  2^  >  1  then 

6  =/iwrap(0,  —  7r,  7r)  0  =arcsin(sin(0))  (50) 

otherwise 

9  =/iwrap(0  -f  7r,  —  7r,  7r)  0  =  arcsin(sin(0)).  (51) 
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